/*
**********************************************************************************
    OpenSURF642 : An TI DM642 implementation of OpenSURF.
    Copyright (C) 2011  Hai Yu

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
    
	This implementation is based on The original OpenSURF library created
	by Christopher Evans.The original work can be found at
		http://www.chrisevansdev.com/computer-vision-opensurf.html
	
    contact: yuhaihai12345@gmail.com	
**********************************************************************************
*/

#include "MatrixInverse.h"

#define  DOUBLE_EPSILON  0.0000000001f
#define  FLOAT_EPSILON   0.0000000001f

void MatrixInverseDouble(double * matrix, double * inv, int rows, int columns)
{
    int i,j;  
    int current_row = 0;    
	
	double * B = (double *)malloc(columns * sizeof(double));
	double * C = (double *)malloc(columns * sizeof(double));
	double * D = (double *)malloc(rows * sizeof(double));
	
    double sum = 0;
    
    double sum_d = 0;
       
    // Calculate A0+ ----- a 1*rows matrix
    for(i = 0; i < columns; i++)
    {
       sum  = sum + matrix[i] * matrix[i];               
    }
    if( sum >= DOUBLE_EPSILON)
    {
       for(i = 0; i < columns; i++)
       {
            inv[i * rows] = matrix[i]/sum;
       }    
    }
    else
    {
       for(i = 0; i < rows; i++)
       {
            inv[i * rows] = 0;
       }          
    }
    
    // Calculate the rest rows of the pseudo inverse matrix
    for(current_row = 1; current_row < rows; current_row ++)
    {
        //calculate D
        for(i = 0; i < current_row; i++)
        {
            sum = 0;
            for(j = 0; j < columns; j++)
            {
                sum += matrix[current_row * columns + j] * inv[j * rows + i];   
            }
            D[i] = sum;                  
        }
        //calculate C
        for(i = 0; i < columns; i++)
        {
            sum = 0;
            for(j = 0; j < current_row; j++)
            {
                sum += D[j] * matrix[i + j * columns];
            }
            C[i] = matrix[current_row * columns + i] - sum;
        }
        //calculate B
        sum = 0;
        for(i = 0;i < columns; i++)
        {
            sum += C[i] * C[i];    
        }
        
        if(sum < DOUBLE_EPSILON)
        {
            //C = 0
            sum_d = 0;
            for(i = 0; i < current_row; i++)
            {
                sum_d += D[i] * D[i];
            }
            sum_d = sum_d + 1;
            
            for(i = 0; i < columns; i++)
            {
                sum = 0;
                for(j = 0; j < current_row; j++)
                {
                    sum = sum + D[j] * inv[i*rows + j];
                }
                B[i] = sum/sum_d;
            }
               
        }
        else
        {
            //C != 0
            for(i = 0; i < columns; i++)
            {
                B[i] = C[i] / sum;
            }
        }
        //calculate inv
        for(j = 0; j < columns; j++)
        {
            for(i = 0; i < current_row; i++)
            {
                inv[j * rows + i] -= B[j] * D[i];
            }
            inv[j * rows + current_row] = B[j];
        }
        
    } 
    free(B);
    free(C);
    free(D);
}


void MatrixInverseFloat(float * matrix, float * inv, int rows, int columns)
{
    int i,j;  
    int current_row = 0;    
	
	float * B = (float *)malloc(columns * sizeof(float));
	float * C = (float *)malloc(columns * sizeof(float));
	float * D = (float *)malloc(rows * sizeof(float));
	
    float sum = 0;
    
    float sum_d = 0;
       
    // Calculate A0+ ----- a 1*rows matrix
    for(i = 0; i < columns; i++)
    {
       sum  = addsp(sum , mpysp(matrix[i] , matrix[i]));               
    }
    if( sum >= FLOAT_EPSILON)
    {
       for(i = 0; i < columns; i++)
       {
            inv[i * rows] = divsp(matrix[i],sum);
       }    
    }
    else
    {
       for(i = 0; i < rows; i++)
       {
            inv[i * rows] = 0;
       }          
    }
    
    // Calculate the rest rows of the pseudo inverse matrix
    for(current_row = 1; current_row < rows; current_row ++)
    {
        //calculate D
        for(i = 0; i < current_row; i++)
        {
            sum = 0;
            for(j = 0; j < columns; j++)
            {
                sum = addsp(sum , mpysp(matrix[current_row * columns + j] , inv[j * rows + i]));   
            }
            D[i] = sum;                  
        }
        //calculate C
        for(i = 0; i < columns; i++)
        {
            sum = 0;
            for(j = 0; j < current_row; j++)
            {
                sum =addsp(sum,  mpysp(D[j] , matrix[i + j * columns]));
            }
            C[i] = subsp(matrix[current_row * columns + i] , sum);
        }
        //calculate B
        sum = 0;
        for(i = 0;i < columns; i++)
        {
            sum = addsp(sum,  mpysp(C[i] , C[i]));    
        }
        
        if(sum < FLOAT_EPSILON)
        {
            //C = 0
            sum_d = 0;
            for(i = 0; i < current_row; i++)
            {
                sum_d = addsp(sum_d, mpysp(D[i] , D[i]));
            }
            sum_d = addsp(sum_d , 1);
            
            for(i = 0; i < columns; i++)
            {
                sum = 0;
                for(j = 0; j < current_row; j++)
                {
                    sum = addsp(sum , mpysp(D[j] , inv[i*rows + j]));
                }
                B[i] = divsp(sum,sum_d);
            }
               
        }
        else
        {
            //C != 0
            for(i = 0; i < columns; i++)
            {
                B[i] = divsp(C[i] , sum);
            }
        }
        //calculate inv
        for(j = 0; j < columns; j++)
        {
            for(i = 0; i < current_row; i++)
            {
                inv[j * rows + i] = subsp(inv[j * rows + i] , mpysp(B[j] , D[i]));
            }
            inv[j * rows + current_row] = B[j];
        }
        
    } 
    free(B);
    free(C);
    free(D);
}
